#pragma once

#include <iostream>
#include <fstream>
#include <random>
#include <ctime>

#include <ceres/ceres.h>


// 定义代价函数  残差三维 参数三维
class myCostFunction : public ceres::SizedCostFunction<3, 3> {
	public:

		myCostFunction(Eigen::Vector3d point, double noise){
            curr_point = point;
            mNoise = noise;
        }
		virtual ~myCostFunction() {}
		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {    
            Eigen::Matrix<double, 3, 1> parameters_0(parameters[0][0], parameters[0][1], parameters[0][2]);
            Eigen::Map<Eigen::Matrix<double, 3, 1>> residual(residuals);
            
            Eigen::Matrix3d cov = Eigen::Vector3d(mNoise*mNoise, mNoise*mNoise, mNoise*mNoise).asDiagonal();  
            Eigen::Matrix<double, 3, 3> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 3, 3>>(cov.inverse()).matrixL().transpose();
            residual = parameters_0 - curr_point;
            residual = sqrt_info * residual;
            if(jacobians != NULL)
            {
                if(jacobians[0] != NULL)
                { 
                    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > J_0(jacobians[0]);
                    J_0 = Eigen::Matrix3d::Identity();
                    J_0 = sqrt_info * J_0;
                }
                
            }  

            return true;
        
        }   
		
        Eigen::Vector3d curr_point;
        double mNoise;
};


// 定义代价函数的计算
class ceres_solver
{
private:
    // 优化变量
    std::vector<double *> point_param;

    // AB两组
    std::vector<Eigen::Vector3d> pointA;
    std::vector<Eigen::Vector3d> pointB;
    std::vector<Eigen::Vector3d> pointTrue;
    std::vector<double> times;

    double beginT = 0;
    double endT = 10;
    double incT = 0.1;

    double k1 = 2.;
    double k2 = 3.;
    double k3 = 1.;

    double NoiseA = 0.2;
    double NoiseB = 0.4;

    // log file
    std::ofstream outTrue;
    std::ofstream pointAFile;
    std::ofstream pointBFile;
    std::ofstream fusionFile;
public:

    void solve(){
        // 构建问题
        ceres::Problem::Options problem_options;
        ceres::Problem problem(problem_options);
        point_param.resize(pointA.size());

        for(auto & pointer : point_param){
            pointer = new double[4];
            pointer[0] = 0;
            pointer[1] = 0;
            pointer[2] = 0;

            problem.AddParameterBlock(pointer, 3);
        }
        // 添加代价块

        for(int j = 0; j < pointA.size(); j++){
            ceres::CostFunction *cost_function = new myCostFunction(pointA[j], NoiseA);  
            problem.AddResidualBlock(cost_function, NULL, point_param[j]);
        }
        for(int j = 0; j < pointB.size(); j++){
            ceres::CostFunction *cost_function = new myCostFunction(pointB[j], NoiseB);  
            problem.AddResidualBlock(cost_function, NULL, point_param[j]);
        }
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;
        options.check_gradients = false;
        options.gradient_check_relative_precision = 1e-4;
        ceres::Solver::Summary summary;

        ceres::Solve(options, &problem, &summary);
        std::cout << std::endl;
        
    }

    ceres_solver(double k1 = 2., double k2 = 3., double k3 = 1.){

        this->pointB.clear();
        this->pointA.clear();
        for(double t = beginT; t < endT; t += incT){
            double TrueX = k1 * t * sin(t);
            double TrueY = k2 * t * cos(t);
            double TrueZ = k3 * t;
            times.push_back(t);
            pointTrue.push_back(Eigen::Vector3d(TrueX, TrueY, TrueZ));

            //产生随机数引擎，采用time作为种子，以确保每次运行程序都会得到不同的结果
            static std::default_random_engine e(time(0));
            static std::normal_distribution<double> nA(0,NoiseA);
            static std::normal_distribution<double> nB(0,NoiseB);
            Eigen::Vector3d nnA(nA(e), nA(e), nA(e));
            Eigen::Vector3d nnB(nB(e), nB(e), nB(e));
            pointA.push_back(Eigen::Vector3d(TrueX, TrueY, TrueZ) + nnA);
            pointB.push_back(Eigen::Vector3d(TrueX, TrueY, TrueZ) + nnB);
        }

        outTrue.open("true.txt");
        pointAFile.open("pointA.txt");
        pointBFile.open("pointB.txt");
        fusionFile.open("fusion.txt");
        
    }
    // log成tum格式
    void logPoints(){

        for(int i = 0; i < pointA.size(); i++){
            outTrue << times[i] << " "
            << pointTrue[i][0] << " "
            << pointTrue[i][1] << " "
            << pointTrue[i][2] << " "
            << 0. << " "
            << 0. << " "
            << 0. << " "
            << 1. << std::endl;

            pointAFile << times[i] << " "
            << pointA[i][0] << " "
            << pointA[i][1] << " "
            << pointA[i][2] << " "
            << 0. << " "
            << 0. << " "
            << 0. << " "
            << 1. << std::endl;

            pointBFile << times[i] << " "
            << pointB[i][0] << " "
            << pointB[i][1] << " "
            << pointB[i][2] << " "
            << 0. << " "
            << 0. << " "
            << 0. << " "
            << 1. << std::endl;

            fusionFile << times[i] << " "
            << point_param[i][0] << " "
            << point_param[i][1] << " "
            << point_param[i][2] << " "
            << 0. << " "
            << 0. << " "
            << 0. << " "
            << 1. << std::endl;
        }
        
    }

    ~ceres_solver(){}
};

